#include "stm32f10x.h"                  // Device header
#include <math.h>
#include "Delay.h"
#include "OLED.h"
#include "Key.h"
#include "MPU6050.h"
#include "MyI2C.h"

void Simple_I2C_Test(void);

//互补滤波计算欧拉角
int16_t ax,ay,az,gx,gy,gz;	//MPU6050返回的加速度，角速度
float roll_g,pitch_g,yaw_g;	//陀螺仪解算的欧拉角
float roll_a,pitch_a;		//加速度计算的欧拉角
float Roll,Pitch,Yaw;		//互补滤波后的欧拉角
float a=0.95;				//互补滤波器系数（改为0.95，更依赖陀螺仪）
float Delya_t=0.005;		//采样周期▲t
float Pi=3.1415927;			//Π

// 添加校准偏移量
int16_t gx_offset = 0, gy_offset = 0, gz_offset = 0;
uint8_t calibrated = 0;     // 校准标志

/**
  * 函    数：MPU6050校准
  * 参    数：无
  * 返 回 值：无
  */
void MPU6050_Calibrate(void)
{
    int32_t gx_sum = 0, gy_sum = 0, gz_sum = 0;
    uint16_t i, num = 100;
    
    // 显示校准提示
    OLED_Clear();
    OLED_ShowString(0,0,"Calibrating...",OLED_8X16);
    OLED_Update();
    
    for(i = 0; i < num; i++)
    {
        MPU6050_GetData(&ax, &ay, &az, &gx, &gy, &gz);
        gx_sum += gx;
        gy_sum += gy;
        gz_sum += gz;
        Delay_ms(5);
    }
    
    gx_offset = gx_sum / num;
    gy_offset = gy_sum / num;
    gz_offset = gz_sum / num;
    calibrated = 1;
    
    // 校准完成提示
    OLED_Clear();
    OLED_ShowString(0,0,"Calibrate Done!",OLED_8X16);
    OLED_Update();
    Delay_ms(500);
}

void MPU6050_Calculate(void)
{
    Delay_ms(5);
    
    // 获取传感器数据，检查是否成功
    if(MPU6050_GetData(&ax,&ay,&az,&gx,&gy,&gz) != 0) {
        // 读取失败，可以设置错误标志或使用上次数据
        return;
    }
    
    // 第一次使用时自动校准
    if(!calibrated) {
        MPU6050_Calibrate();
        return;
    }
    
    // 减去零点偏移
    gx -= gx_offset;
    gy -= gy_offset;
    gz -= gz_offset;
    
    // 单位转换
    float gx_dps = gx / 16.4f;      // ±2000dps，16.4LSB/(dps)
    float gy_dps = gy / 16.4f;
    float gz_dps = gz / 16.4f;
    
    float ax_g = ax / 16384.0f;     // ±2g，16384LSB/g
    float ay_g = ay / 16384.0f;
    float az_g = az / 16384.0f;
    
    //通过陀螺仪解算欧拉角
    roll_g = Roll + gx_dps * Delya_t;
    pitch_g = Pitch + gy_dps * Delya_t;
    yaw_g = Yaw + gz_dps * Delya_t;
    
    //通过加速度计算欧拉角
    float roll_rad = atan2(ay_g, az_g);
    float pitch_rad = atan2(-ax_g, sqrt(ay_g*ay_g + az_g*az_g));
    
    roll_a = roll_rad * 180/Pi;
    pitch_a = pitch_rad * 180/Pi;
    
    //通过互补滤波器进行融合
    Roll = a * roll_g + (1 - a) * roll_a;
    Pitch = a * pitch_g + (1 - a) * pitch_a;
    Yaw = a * yaw_g;
}


void Show_Balance_UI(void)
{
	
    OLED_ShowImage(0,0,16,16,Image_Return);
    OLED_Printf(0,16,OLED_8X16,"Roll: %.2f",Roll);
    OLED_Printf(0,32,OLED_8X16,"Pitch:%.2f",Pitch);
    OLED_Printf(0,48,OLED_8X16,"Yaw:  %.2f",Yaw);
	
	 // 调试信息：显示原始加速度值
	/*
    OLED_Printf(64,0,OLED_6X8,"ax:%d",ax);
    OLED_Printf(64,8,OLED_6X8,"ay:%d",ay);
    OLED_Printf(64,16,OLED_6X8,"az:%d",az);
	*/
}

extern uint8_t KeyNum;
int8_t Balance(void)
{
	Simple_I2C_Test();
	// 添加MPU6050状态检查
	MPU6050_Init();
    uint8_t mpu_id = MPU6050_GetID();
    OLED_Clear();
    OLED_Printf(0,0,OLED_8X16,"MPU ID: 0x%02X", mpu_id);
    OLED_Update();
    Delay_ms(2000);
    
    // 正常ID应该是0x68，如果不是则有问题
    if(mpu_id != 0x68) {
        OLED_Clear();
        OLED_ShowString(0,0,"MPU6050 Error!",OLED_8X16);
        OLED_Printf(0,16,OLED_8X16,"ID:0x%02X",mpu_id);
        OLED_ShowString(0,32,"Check Connection",OLED_8X16);
        OLED_Update();
        Delay_ms(3000);
        return -1; // 返回错误
    }
    
    // 重置角度值，避免上次数据影响
    Roll = 0;
    Pitch = 0; 
    Yaw = 0;
    
    while(1)
    {
        KeyNum=Key_GetNum();
        if(KeyNum==3)
        {
            OLED_Clear();
            OLED_Update();
            return 0;
        }
        // 添加按键4手动重新校准
        if(KeyNum==1) {
            MPU6050_Calibrate();
        }
        
        OLED_Clear();
        MPU6050_Calculate();
        Show_Balance_UI();
        // 显示校准状态
        if(calibrated) {
            OLED_ShowString(80,0,"CAL",OLED_6X8);
        }
        OLED_ReverseArea(0,0,16,16);
        OLED_Update();
    }
}
// 在 Balance.c 中添加这个简单测试函数
void Simple_I2C_Test(void)
{
    uint8_t found_devices = 0;
    
    OLED_Clear();
    OLED_ShowString(0,0,"Simple I2C Test",OLED_8X16);
    OLED_Update();
    Delay_ms(1000);
    
    // 扫描I2C设备
    found_devices = MyI2C_ScanDevices();
    
    if(found_devices == 0) {
        OLED_Clear();
        OLED_ShowString(0,0,"No I2C Devices!",OLED_8X16);
        OLED_ShowString(0,16,"Check:",OLED_8X16);
        OLED_ShowString(0,32,"1. Power",OLED_8X16);
        OLED_ShowString(0,48,"2. Pull-up Resistors",OLED_8X16);
        OLED_Update();
        Delay_ms(5000);
    }
}

/******************************水平仪*****************************************/
void Show_Gradienter_UI(void)
{
	MPU6050_Calculate();
	OLED_DrawCircle(64,32,30,0);
	OLED_DrawCircle(64-Roll,32+Pitch,4,1);
}

int Gradienter(void)
{
	MPU6050_Init();
	while(1)
	{
		KeyNum=Key_GetNum();
		if(KeyNum==3)
		{
			OLED_Clear();
			OLED_Update();
			return 0;
		}
		OLED_Clear();
		Show_Gradienter_UI();
		OLED_Update();
	}
}
